#include "wpml.h"
#include <cstdint>
#include <chrono>
#include <map>
#include <sstream>
#include <iomanip>

namespace wpml
{
    // 枚举到字符串的转换函数
    std::string flyToWaylineModeToString(FlyToWaylineMode mode)
    {
        static const std::map<FlyToWaylineMode, std::string> modeMap = {
            {FlyToWaylineMode::Safely, "safely"},
            {FlyToWaylineMode::PointToPoint, "pointToPoint"}};
        return modeMap.at(mode);
    }

    std::string finishActionToString(FinishAction action)
    {
        static const std::map<FinishAction, std::string> actionMap = {
            {FinishAction::GoHome, "goHome"},
            {FinishAction::NoAction, "noAction"},
            {FinishAction::AutoLand, "autoLand"},
            {FinishAction::GotoFirstWaypoint, "gotoFirstWaypoint"}};
        return actionMap.at(action);
    }

    std::string exitOnRCLostToString(ExitOnRCLost action)
    {
        static const std::map<ExitOnRCLost, std::string> actionMap = {
            {ExitOnRCLost::GoContinue, "goContinue"},
            {ExitOnRCLost::ExecuteLostAction, "executeLostAction"}};
        return actionMap.at(action);
    }

    std::string executeRCLostActionToString(ExecuteRCLostAction action)
    {
        static const std::map<ExecuteRCLostAction, std::string> actionMap = {
            {ExecuteRCLostAction::GoBack, "goBack"},
            {ExecuteRCLostAction::Landing, "landing"},
            {ExecuteRCLostAction::Hover, "hover"}};
        return actionMap.at(action);
    }

    std::string templateTypeToString(TemplateType type)
    {
        static const std::map<TemplateType, std::string> typeMap = {
            {TemplateType::Waypoint, "waypoint"},
            {TemplateType::Mapping2d, "mapping2d"},
            {TemplateType::Mapping3d, "mapping3d"},
            {TemplateType::MappingStrip, "mappingStrip"}};
        return typeMap.at(type);
    }

    std::string coordinateModeToString(CoordinateMode mode)
    {
        static const std::map<CoordinateMode, std::string> modeMap = {
            {CoordinateMode::WGS84, "WGS84"}};
        return modeMap.at(mode);
    }

    std::string heightModeToString(HeightMode mode)
    {
        static const std::map<HeightMode, std::string> modeMap = {
            {HeightMode::EGM96, "EGM96"},
            {HeightMode::RelativeToStartPoint, "relativeToStartPoint"},
            {HeightMode::AboveGroundLevel, "aboveGroundLevel"},
            {HeightMode::RealTimeFollowSurface, "realTimeFollowSurface"}};
        return modeMap.at(mode);
    }

    std::string positioningTypeToString(PositioningType type)
    {
        static const std::map<PositioningType, std::string> typeMap = {
            {PositioningType::GPS, "GPS"},
            {PositioningType::RTKBaseStation, "RTKBaseStation"},
            {PositioningType::QianXun, "QianXun"},
            {PositioningType::Custom, "Custom"}};
        return typeMap.at(type);
    }

    std::string gimbalPitchModeToString(GimbalPitchMode mode)
    {
        static const std::map<GimbalPitchMode, std::string> modeMap = {
            {GimbalPitchMode::Manual, "manual"},
            {GimbalPitchMode::UsePointSetting, "usePointSetting"}};
        return modeMap.at(mode);
    }

    std::string waypointTurnModeToString(WaypointTurnMode mode)
    {
        static const std::map<WaypointTurnMode, std::string> modeMap = {
            {WaypointTurnMode::CoordinateTurn, "coordinateTurn"},
            {WaypointTurnMode::ToPointAndStopWithDiscontinuityCurvature, "toPointAndStopWithDiscontinuityCurvature"},
            {WaypointTurnMode::ToPointAndStopWithContinuityCurvature, "toPointAndStopWithContinuityCurvature"},
            {WaypointTurnMode::ToPointAndPassWithContinuityCurvature, "toPointAndPassWithContinuityCurvature"}};
        return modeMap.at(mode);
    }

    std::string waypointHeadingModeToString(WaypointHeadingMode mode)
    {
        static const std::map<WaypointHeadingMode, std::string> modeMap = {
            {WaypointHeadingMode::Fixed, "fixed"},
            {WaypointHeadingMode::FollowWayline, "followWayline"}};
        return modeMap.at(mode);
    }

    std::string toWaylines(const wpml::Document &doc)
    {
        std::ostringstream oss;
        oss << std::fixed << std::setprecision(15);

        // XML 声明和根元素
        oss << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n";
        oss << "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:wpml=\"http://www.dji.com/wpmz/1.0.0\">\n";
        oss << "  <Document>\n";

        // missionConfig 部分
        oss << "    <wpml:missionConfig>\n";
        oss << "      <wpml:flyToWaylineMode>" << flyToWaylineModeToString(doc.missionConfig.flyToWaylineMode) << "</wpml:flyToWaylineMode>\n";
        oss << "      <wpml:finishAction>" << finishActionToString(doc.missionConfig.finishAction) << "</wpml:finishAction>\n";
        oss << "      <wpml:exitOnRCLost>" << exitOnRCLostToString(doc.missionConfig.exitOnRCLost) << "</wpml:exitOnRCLost>\n";
        oss << "      <wpml:takeOffSecurityHeight>" << doc.missionConfig.takeOffSecurityHeight << "</wpml:takeOffSecurityHeight>\n";
        oss << "      <wpml:globalTransitionalSpeed>" << doc.missionConfig.globalTransitionalSpeed << "</wpml:globalTransitionalSpeed>\n";
        oss << "      <wpml:droneInfo>\n";
        oss << "        <wpml:droneEnumValue>" << doc.missionConfig.droneInfo.droneEnumValue << "</wpml:droneEnumValue>\n";
        oss << "        <wpml:droneSubEnumValue>" << doc.missionConfig.droneInfo.droneSubEnumValue << "</wpml:droneSubEnumValue>\n";
        oss << "      </wpml:droneInfo>\n";
        oss << "      <wpml:payloadInfo>\n";
        oss << "        <wpml:payloadEnumValue>" << doc.missionConfig.payloadInfo.payloadEnumValue << "</wpml:payloadEnumValue>\n";
        oss << "        <wpml:payloadSubEnumValue>" << doc.missionConfig.payloadInfo.payloadSubEnumValue << "</wpml:payloadSubEnumValue>\n";
        oss << "        <wpml:payloadPositionIndex>" << doc.missionConfig.payloadInfo.payloadPositionIndex << "</wpml:payloadPositionIndex>\n";
        oss << "      </wpml:payloadInfo>\n";
        oss << "    </wpml:missionConfig>\n";

        // Folder 部分
        oss << "    <Folder>\n";
        oss << "      <wpml:templateId>" << doc.wayline.templateId << "</wpml:templateId>\n";
        oss << "      <wpml:executeHeightMode>" << heightModeToString(static_cast<wpml::HeightMode>(doc.wayline.executeHeightMode_b)) << "</wpml:executeHeightMode>\n";
        oss << "      <wpml:waylineId>" << doc.wayline.waylineId_b << "</wpml:waylineId>\n";
        oss << "      <wpml:distance>" << doc.wayline.distance_b << "</wpml:distance>\n";
        oss << "      <wpml:duration>" << doc.wayline.duration_b << "</wpml:duration>\n";
        oss << "      <wpml:autoFlightSpeed>" << doc.wayline.autoFlightSpeed << "</wpml:autoFlightSpeed>\n";

        // 航点部分
        for (size_t i = 0; i < doc.wayline.waypoints.size(); ++i)
        {
            const auto &waypoint = doc.wayline.waypoints[i];
            oss << "      <Placemark>\n";
            oss << "        <Point>\n";
            oss << "          <coordinates>\n";
            oss << "            " << waypoint.longitude << "," << waypoint.latitude << "\n";
            oss << "          </coordinates>\n";
            oss << "        </Point>\n";
            oss << "        <wpml:index>" << i << "</wpml:index>\n";
            oss << "        <wpml:executeHeight>" << waypoint.executeHeight_b << "</wpml:executeHeight>\n";
            oss << "        <wpml:waypointSpeed>" << waypoint.waypointSpeed_b << "</wpml:waypointSpeed>\n";
            oss << "        <wpml:waypointHeadingParam>\n";
            oss << "          <wpml:waypointHeadingMode>" << waypointHeadingModeToString(waypoint.headingParam_b.waypointHeadingMode) << "</wpml:waypointHeadingMode>\n";
            oss << "          <wpml:waypointHeadingAngle>" << waypoint.headingParam_b.waypointHeadingAngle << "</wpml:waypointHeadingAngle>\n";
            oss << "          <wpml:waypointPoiPoint>" << waypoint.headingParam_b.waypointPoiPoint << "</wpml:waypointPoiPoint>\n";
            oss << "          <wpml:waypointHeadingAngleEnable>" << (waypoint.headingParam_b.waypointHeadingAngleEnable ? "1" : "0") << "</wpml:waypointHeadingAngleEnable>\n";
            oss << "        </wpml:waypointHeadingParam>\n";
            oss << "        <wpml:waypointTurnParam>\n";
            oss << "          <wpml:waypointTurnMode>" << waypointTurnModeToString(waypoint.turnParam_b.waypointTurnMode) << "</wpml:waypointTurnMode>\n";
            oss << "          <wpml:waypointTurnDampingDist>" << waypoint.turnParam_b.waypointTurnDampingDist << "</wpml:waypointTurnDampingDist>\n";
            oss << "        </wpml:waypointTurnParam>\n";
            oss << "        <wpml:useStraightLine>" << (waypoint.useStraightLine_b ? "1" : "0") << "</wpml:useStraightLine>\n";
            oss << "      </Placemark>\n";
        }

        // 结束标签
        oss << "    </Folder>\n";
        oss << "  </Document>\n";
        oss << "</kml>\n";

        return oss.str();
    }

    std::string toTemplate(const wpml::Document &doc)
    {
        std::ostringstream oss;
        oss << std::fixed << std::setprecision(15);

        // XML 声明和根元素
        oss << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n";
        oss << "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:wpml=\"http://www.dji.com/wpmz/1.0.0\">\n";
        oss << "  <Document>\n";

        // 添加创建和更新时间
        oss << "    <wpml:createTime>" << doc.createTime << "</wpml:createTime>\n";
        oss << "    <wpml:updateTime>" << doc.updateTime << "</wpml:updateTime>\n";

        // missionConfig 部分
        oss << "    <wpml:missionConfig>\n";
        oss << "      <wpml:flyToWaylineMode>" << flyToWaylineModeToString(doc.missionConfig.flyToWaylineMode) << "</wpml:flyToWaylineMode>\n";
        oss << "      <wpml:finishAction>" << finishActionToString(doc.missionConfig.finishAction) << "</wpml:finishAction>\n";
        oss << "      <wpml:exitOnRCLost>" << exitOnRCLostToString(doc.missionConfig.exitOnRCLost) << "</wpml:exitOnRCLost>\n";
        oss << "      <wpml:takeOffSecurityHeight>" << doc.missionConfig.takeOffSecurityHeight << "</wpml:takeOffSecurityHeight>\n";
        oss << "      <wpml:globalTransitionalSpeed>" << doc.missionConfig.globalTransitionalSpeed << "</wpml:globalTransitionalSpeed>\n";
        oss << "      <wpml:droneInfo>\n";
        oss << "        <wpml:droneEnumValue>" << doc.missionConfig.droneInfo.droneEnumValue << "</wpml:droneEnumValue>\n";
        oss << "        <wpml:droneSubEnumValue>" << doc.missionConfig.droneInfo.droneSubEnumValue << "</wpml:droneSubEnumValue>\n";
        oss << "      </wpml:droneInfo>\n";
        oss << "      <wpml:payloadInfo>\n";
        oss << "        <wpml:payloadEnumValue>" << doc.missionConfig.payloadInfo.payloadEnumValue << "</wpml:payloadEnumValue>\n";
        oss << "        <wpml:payloadSubEnumValue>" << doc.missionConfig.payloadInfo.payloadSubEnumValue << "</wpml:payloadSubEnumValue>\n";
        oss << "        <wpml:payloadPositionIndex>" << doc.missionConfig.payloadInfo.payloadPositionIndex << "</wpml:payloadPositionIndex>\n";
        oss << "      </wpml:payloadInfo>\n";
        oss << "    </wpml:missionConfig>\n";

        // Folder 部分
        oss << "    <Folder>\n";
        oss << "      <wpml:templateType>" << templateTypeToString(doc.wayline.templateType_a) << "</wpml:templateType>\n";
        oss << "      <wpml:useGlobalTransitionalSpeed>" << (doc.wayline.useGlobalTransitionalSpeed_a ? "1" : "0") << "</wpml:useGlobalTransitionalSpeed>\n";
        oss << "      <wpml:templateId>" << doc.wayline.templateId << "</wpml:templateId>\n";

        // 坐标系参数
        oss << "      <wpml:waylineCoordinateSysParam>\n";
        oss << "        <wpml:coordinateMode>" << coordinateModeToString(doc.wayline.coordinateSysParam_a.coordinateMode) << "</wpml:coordinateMode>\n";
        oss << "        <wpml:heightMode>" << heightModeToString(doc.wayline.coordinateSysParam_a.heightMode) << "</wpml:heightMode>\n";
        oss << "        <wpml:globalHeight>" << doc.wayline.coordinateSysParam_a.globalHeight << "</wpml:globalHeight>\n";
        oss << "        <wpml:positioningType>" << positioningTypeToString(doc.wayline.coordinateSysParam_a.positioningType) << "</wpml:positioningType>\n";
        oss << "      </wpml:waylineCoordinateSysParam>\n";

        // 其他参数
        oss << "      <wpml:autoFlightSpeed>" << doc.wayline.autoFlightSpeed << "</wpml:autoFlightSpeed>\n";
        oss << "      <wpml:transitionalSpeed>" << doc.wayline.transitionalSpeed_a << "</wpml:transitionalSpeed>\n";
        oss << "      <wpml:caliFlightEnable>" << (doc.wayline.caliFlightEnable_a ? "1" : "0") << "</wpml:caliFlightEnable>\n";
        oss << "      <wpml:gimbalPitchMode>" << gimbalPitchModeToString(doc.wayline.gimbalPitchMode_a) << "</wpml:gimbalPitchMode>\n";

        // 全局航点朝向参数
        oss << "      <wpml:globalWaypointHeadingParam>\n";
        oss << "        <wpml:waypointHeadingMode>" << waypointHeadingModeToString(doc.wayline.globalHeadingParam_a.waypointHeadingMode) << "</wpml:waypointHeadingMode>\n";
        oss << "        <wpml:waypointHeadingAngle>" << doc.wayline.globalHeadingParam_a.waypointHeadingAngle << "</wpml:waypointHeadingAngle>\n";
        oss << "        <wpml:waypointPoiPoint>" << doc.wayline.globalHeadingParam_a.waypointPoiPoint << "</wpml:waypointPoiPoint>\n";
        oss << "        <wpml:waypointHeadingAngleEnable>" << (doc.wayline.globalHeadingParam_a.waypointHeadingAngleEnable ? "1" : "0") << "</wpml:waypointHeadingAngleEnable>\n";
        oss << "      </wpml:globalWaypointHeadingParam>\n";

        // 全局航点转弯模式
        oss << "      <wpml:globalWaypointTurnMode>" << waypointTurnModeToString(doc.wayline.globalWaypointTurnMode) << "</wpml:globalWaypointTurnMode>\n";

        // 航点部分 (使用 waypoints)
        for (size_t i = 0; i < doc.wayline.waypoints.size(); ++i)
        {
            const auto &waypoint = doc.wayline.waypoints[i];
            oss << "      <Placemark>\n";
            oss << "        <Point>\n";
            oss << "          <coordinates>\n";
            oss << "            " << waypoint.longitude << "," << waypoint.latitude << "\n";
            oss << "          </coordinates>\n";
            oss << "        </Point>\n";
            oss << "        <wpml:index>" << i << "</wpml:index>\n";
            oss << "        <wpml:ellipsoidHeight>" << waypoint.ellipsoidHeight_a << "</wpml:ellipsoidHeight>\n";
            oss << "        <wpml:height>" << waypoint.height_a << "</wpml:height>\n";
            oss << "        <wpml:useGlobalHeight>" << (waypoint.useGlobalHeight_a ? "1" : "0") << "</wpml:useGlobalHeight>\n";
            oss << "        <wpml:useGlobalSpeed>" << (waypoint.useGlobalSpeed_a ? "1" : "0") << "</wpml:useGlobalSpeed>\n";
            oss << "        <wpml:useGlobalHeadingParam>" << (waypoint.useGlobalHeadingParam_a ? "1" : "0") << "</wpml:useGlobalHeadingParam>\n";
            oss << "        <wpml:useGlobalTurnParam>" << (waypoint.useGlobalTurnParam_a ? "1" : "0") << "</wpml:useGlobalTurnParam>\n";
            oss << "      </Placemark>\n";
        }

        // 结束标签
        oss << "    </Folder>\n";
        oss << "  </Document>\n";
        oss << "</kml>\n";

        return oss.str();
    }
}